#include "grid.h"
#include <math.h>
#include <QMessageBox>

bool lessHorizontalLine(const QLineF & l1, const QLineF & l2){
    return l1.y1()<l2.y1();
}

bool lessVerticalLine(const QLineF & l1, const QLineF & l2){
    return l1.x1()<l2.x2();
}

void GridPoint::addNeighbor(GridPoint* n){
    if (!neighbors.contains(n))
        neighbors.append(n);
}

Grid::~Grid(){
}

void Grid::connectIfReachable(GridPoint * p1,GridPoint * p2){
 bool reachable=true;
 QLineF line(*p1,*p2);
 for (int k=0;k<blocks.size() && reachable;k++) {
     reachable = reachable && !intersects(line,blocks[k]);
 }
 if (reachable) {
     p1->addNeighbor(p2);
     p2->addNeighbor(p1);
 }
}

bool fuzzyCompare(const QPointF & p1,const QPointF & p2) {
  bool dx=fabs(p1.x()-p2.x())<0.001;
  bool dy=fabs(p1.y()-p2.y())<0.001;
  return dx && dy;
}

void Grid::build(){
    if (built)
        return;
    built=true;
    //Adding grid lines
    gridHLines << makeHLine(bounds.top());
    gridVLines << makeVLine(bounds.left());
    for (int i = 0; i < blocks.size(); i++){
        gridVLines << makeVLine( blocks[i].left()   -margin);
        gridVLines << makeVLine( blocks[i].right()  +margin);
        gridHLines << makeHLine( blocks[i].top()    -margin);
        gridHLines << makeHLine( blocks[i].bottom() +margin);
    }
    gridHLines << makeHLine(bounds.bottom());
    gridVLines << makeVLine(bounds.right());

    gridHLines << makeHLine(start.y());
    gridVLines << makeVLine(start.x());

    gridHLines << makeHLine(goal.y());
    gridVLines << makeVLine(goal.x());
    //Sorting them by position
    qSort(gridHLines.begin(),gridHLines.end(),lessHorizontalLine);
    qSort(gridVLines.begin(),gridVLines.end(),lessVerticalLine);

    //Adding grid points and introducing them to reachable neighbors;
	for (int i = 0; i < gridHLines.size(); i++){		
		gridPoints.append( QList<GridPoint*>() );
        for (int j = 0; j < gridVLines.size(); j++){	
			gridPoints[i].append( new GridPoint(QPointF()) );
            gridHLines[i].intersect(gridVLines[j],gridPoints[i][j]);
            if (i > 0)
                    connectIfReachable(gridPoints[i][j],gridPoints[i-1][j]);
            if (j > 0)
                    connectIfReachable(gridPoints[i][j],gridPoints[i][j-1]);
        }
	}
	for (int i = 0; i < gridPoints.size() && !(g_start && g_goal); i++)	
        for (int j = 0; j < gridPoints[i].size() && !(g_start && g_goal); j++){
                        GridPoint * test=gridPoints[i][j];
                        if ( fuzzyCompare(*test, start))
                                g_start = test;
                        if ( fuzzyCompare(*test , goal) )
                                g_goal = test;
		}
        if (g_start)
            if (!fuzzyCompare(*g_start,start))
                QMessageBox::critical(NULL,"Fail!","Incorrect start!");
        if (g_start)
            if (!fuzzyCompare(*g_goal,goal))
                QMessageBox::critical(NULL,"Fail!","Incorrect goal!");
}


/*! Is on line in one dimension
    \param[in] y second coordinates, which is checked with
    \param[in] ly line y
    \param[in] x  point first checked coordinate, which is laid
    \param[in] lx1
    \param[in] lx2
 */
inline bool isOnLine1D(const float y, const float ly, const float x,  float lx1,  float lx2) {
    if (lx1>lx2) {
        float tmp=lx1;
        lx1=lx2;
        lx2=tmp;
    }
    bool  isSecondCoordinateEq=fabs(y-ly)<0.001;
    bool  isEqToFirstPoint=fabs(x-lx1)<0.001;
    bool  isEqToSecondPoint=fabs(x-lx2)<0.001;
    bool  isWithinPoint= lx1<=x && x<=lx2;
    return isSecondCoordinateEq && (isEqToFirstPoint || isEqToSecondPoint || isWithinPoint);
}

/*! Determines, whether point is on line (used for rectangle)
 */
bool isOnLine(const QPointF & p, const QLineF & l) {
    if (fabs(l.x1()-l.x2())<0.001)
        return isOnLine1D(p.x(),l.x1(),p.y(),l.y1(),l.y2());
    return isOnLine1D(p.y(),l.y1(),p.x(),l.x1(),l.x2());
}
/*! Determines, whether point is on rect
 */
bool isOnRect(const QPointF & p, const QRectF & r) {
   bool isOnTop=isOnLine(p,QLineF(r.topLeft(),r.topRight()));
   bool isOnBottom=isOnLine(p,QLineF(r.bottomLeft(),r.bottomRight()));
   bool isOnLeft=isOnLine(p,QLineF(r.topLeft(),r.bottomLeft()));
   bool isOnRight=isOnLine(p,QLineF(r.topRight(),r.bottomRight()));
   return isOnTop || isOnBottom ||  isOnLeft || isOnRight;
}
/*! Determines, whether point is within rect
 */
bool isWithinRect(const QPointF & p, const QRectF & r) {
    bool isWithinX=  r.left()<=p.x() && p.x()<=r.right();
    bool isWithinY=  r.top()<=p.y() && p.y()<=r.bottom();
    bool isOn=isOnRect(p,r);
    return (isWithinX && isWithinY) && !isOn;
}

bool Grid::isWithinPoint(const QPointF & p) {
  bool isWithin=false;
  for (int i=0;i<blocks.size();i++)
      isWithin=isWithin || isWithinRect(p,blocks[i]);
  return isWithin;
}

float       GridPoint::dist(GridPoint * g) {
    float dx=this->x()-g->x();
    float dy=this->y()-g->y();
    return sqrt(dx*dx+dy*dy);
}

float GridPoint::g()
{
  GridPoint * cur=this;
  GridPoint * prev=cur->previous;
  float len=0;
  while (prev!=NULL) {
    len+=cur->dist(prev);
    cur=prev;
    prev=cur->previous;
  }
  return len;
}

float GridPoint::h(GridPoint * goal) {
    float dx=this->x()-goal->x();
    float dy=this->y()-goal->y();
    return fabs(dx)+fabs(dy);
}
float GridPoint::f(GridPoint * goal) {
    return g()+h(goal);
}


void Grid::addToOpenList(QVector<GridPoint *> & open, GridPoint * point, GridPoint * cur) {
    if (open.contains(point)) {
        if (point->g()>cur->g()+cur->dist(point)) {
            point->setPrevious(cur);
            open.remove(open.indexOf(point));
        }
        else return;
    }
    else point->setPrevious(cur);

    int pos=0;
    bool found=false;
    for (pos=0;pos<open.size() && !found; pos++) {
        if (open[pos]->f(g_goal)>point->f(g_goal)) {
            found=true;
        }
    }
    --pos;
    if (found) {
        open.insert(pos,point);
    }
    else
        open<<point;
}

QVector<QPointF> Grid::getRoughPath(){
    QVector<QPointF> result;
    //Open list of used vertexes
    QVector<GridPoint *> open;
    //Closed list of visited vertexes
    QVector<GridPoint *> closed;
    //Add starting point to list
    open<<g_start;
    //Set first point
    g_start->setPrevious(NULL);
    //Flag determines, whether we are found some way
    bool found=false;
    GridPoint * cur=NULL;
    while(open.size() && !found) {
        //Select first point and add it to end
        cur=open[0];
        open.remove(0);
        closed.append(cur);
        if (cur==g_goal) {
            found=true;
        }
        else {
            //Scan neibourghs
            for (int i=0;i<cur->neighborsCount();i++) {
                GridPoint * candidate=(*cur)[i];
                //If closed list contains candidate
                if (closed.contains(candidate)) {
                    //If path is bigger than we found, set it
                    if (candidate->g()>candidate->dist(cur)+cur->g()) {
                        candidate->setPrevious(cur);
                    }
                }
                else {
                    //Insert candidate to ascending list
                    addToOpenList(open,candidate,cur);
                }
            }
        }
    }

    if (found) {
        if (cur!=g_goal) {
            QMessageBox::critical(NULL,"Not consistent","Last point is not goal!");
        }
        while(cur) {
            result.insert(0,*cur);
            if (cur->getPrevious()==NULL && cur!=g_start)
                QMessageBox::critical(NULL,"Not consistent","First point is not start!");
            cur=cur->getPrevious();
        }
    }
    return result;
}

QVector<QPointF> Grid::getPath()
{
  build();
  QVector<QPointF> result;
  //If start is goal are within rects we fail instantly
  if (isWithinPoint(start) || isWithinPoint(goal)) {
      return result;
  }
  QVector<QPointF> roughPath=getRoughPath();
  if (roughPath.size())
  {
   result=optimizePath(roughPath);
  }
  return result;
}


enum Direction {
    D_LEFT=0,
    D_RIGHT=1,
    D_TOP=2,
    D_BOTTOM=3
};

Direction getDirection(const QPointF & start, const QPointF & end) {
    if (fabs(start.x()-end.x())<0.001) {
        if (end.y()<start.y())
            return D_TOP;
        else
            return D_BOTTOM;
    }
    if (end.x()>start.x())
        return D_RIGHT;
    return D_LEFT;
}

/** Reduces points in line, removing odd points
  */
void reducePoints(QVector<QPointF> & p) {
    for (int i=2;i<p.size();i++) {
        if (getDirection(p[i-2],p[i-1])==getDirection(p[i-1],p[i])) {
            p.remove(i-1);
            --i;
        }
    }
}


bool Grid::intersects(const QPointF & start, const QPointF & end) {
    bool result=false;
    QLineF line(start,end);
    for (int k=0;k<blocks.size();k++) {
        result = result || intersects(line,blocks[k]);
    }
    return result;
}


bool Grid::intersects(const QVector<QPointF> & line) {
    bool result=false;
    for (int i=1;i<line.size();i++)
        result=result || intersects(line[i-1],line[i]);
    return result;
}

void Grid::tryOptimize(const Polyline & polyline) {
    for (int i=3;i<polyline.size();i++) {
        QVector<QPointF> tmp=polyline;
        bool handled=false;

        Direction dir1=getDirection(tmp[i-3],tmp[i-2]);
        Direction dir2=getDirection(tmp[i-1],tmp[i]);

        if (dir1==dir2) {
            handled=true;
            if ((dir1==D_LEFT  || dir1==D_RIGHT)) {
                tmp[i-1].setX(tmp[i-3].x());
                tmp.remove(i-2);
            }
            if ((dir1==D_TOP || dir1==D_BOTTOM)) {
                tmp[i-1].setY(tmp[i-3].y());
                tmp.remove(i-2);
            }
        }

        //If done something, try save changes
        if (handled) {
            reducePoints(tmp);
            if (!intersects(tmp))
                this->polylines->append(tmp);
        }
    }
}


QVector<QPointF> Grid::optimizePath(const QVector<QPointF> & path){
    QVector<QPointF> result = path;
    /*! /TODO: Recursive optimization function should be placed here
    */
    reducePoints(result);

    /*! Resulting vector of optimized polylines
     */
    QVector<Polyline> optimization_vector;
    optimization_vector<<result;
    //Flag, when we should stop
    bool finish=false;
    while(!finish) {
       QVector<Polyline> optimized;
       polylines=&optimized;
       for (int i=0;i<optimization_vector.size();i++) {
           tryOptimize(optimization_vector[i]);
       }
       if (optimized.size())
           optimization_vector=optimized;
       else
           finish=true;
    }
    return optimization_vector[0];
}


